/**
* Copyright (C) 2019 Hirain Technologies
* License: Modified BSD Software License Agreement
* Author: Feng DING
* Description:
*/

#include "dbc_canmsg_pack.h"
#include <math.h>
#include <iostream>

#define BITCALCULATEPACK(type) \
	data[startIndex] = data[startIndex] | (uint8_T)((uint8_T)((type)(packedValue & (type)(((type)(1)) << i)) >> i) << leftShift);

#define TYPECALCULATEBITUNPACK(type)       \
	if (s.dataType)                        \
	{                                      \
		for (int i = 0; i < s.length; i++) \
		{                                  \
			BITCALCULATEPACK(type);        \
			leftShift++;                   \
			if (leftShift == 8)            \
			{                              \
				leftShift = 0;             \
				startIndex++;              \
			}                              \
		}                                  \
	}                                      \
	else                                   \
	{                                      \
		for (int i = 0; i < s.length; i++) \
		{                                  \
			BITCALCULATEPACK(type);        \
			leftShift++;                   \
			if (leftShift == 8)            \
			{                              \
				leftShift = 0;             \
				startIndex--;              \
			}                              \
		}                                  \
	}

#define PACKVALUEUNSIGNED(type)          \
	type packedValue;                    \
	if (outValue > (real64_T)(max))      \
	{                                    \
		packedValue = (type)max;         \
	}                                    \
	else if (outValue < (real64_T)(min)) \
	{                                    \
		packedValue = (type)min;         \
	}                                    \
	else                                 \
	{                                    \
		packedValue = (type)(outValue);  \
	}                                    \
	TYPECALCULATEBITUNPACK(type)

#define PACKVALUESIGNED(type)              \
	type packedValue;                      \
	type scaledValue;                      \
	scaledValue = (type)(outValue);        \
	if (scaledValue > (type)(max))         \
	{                                      \
		packedValue = max;                 \
	}                                      \
	else if (scaledValue < (type)(min))    \
	{                                      \
		packedValue = (min);               \
	}                                      \
	else                                   \
	{                                      \
		packedValue = (type)(scaledValue); \
	}                                      \
	TYPECALCULATEBITUNPACK(type)

#define DELTA 0.0001

namespace can_util
{

int packCanmsg(const Message &m, const size_t &valueSize, const double *value, Canmsg *msg)
{
	// if the message has the correct number of signals
	if (valueSize != m.signals.size())
	{
		printf("[%ld] value given error\n", m.id);
		return SIGNAL_SIZE_MISMATCH;
	}
	msg->id = m.id;
	msg->length = m.length;
	int index = 0;
	std::for_each(m.signals.begin(), m.signals.end(), [&](Signal s) {
		packSignal(s, value[index], msg->data);
		index++;
	});
	return PACK_UNPACK_SUCCESS;
}
int packCanmsg(const Message &m, const size_t &valueSize, const double *value, can_frame &msg)
{
	// if the message has the correct number of signals
	if (valueSize != m.signals.size())
	{
		printf("[%ld] value given error\n", m.id);
		return SIGNAL_SIZE_MISMATCH;
	}
	msg.can_id = m.id;
	msg.can_dlc = m.length;
	int index = 0;
	std::for_each(m.signals.begin(), m.signals.end(), [&](Signal s) {
		packSignal(s, value[index], msg.data);
		index++;
	});
	return PACK_UNPACK_SUCCESS;
}
int packCanmsg(const Message &m, const std::vector<double> &signals_value, can_frame &msg)
{
	// if the message has the correct number of signals
	if (signals_value.size() != m.signals.size())
	{
		printf("[%ld] value given error\n", m.id);
		return SIGNAL_SIZE_MISMATCH;
	}
	msg.can_id = m.id;
	msg.can_dlc = m.length;
	int index = 0;
	std::for_each(m.signals.begin(), m.signals.end(), [&](Signal s) {
		packSignal(s, signals_value[index], msg.data);
		index++;
	});
	return PACK_UNPACK_SUCCESS;
}
void packSignal(const Signal &s, const double &value, uint8_T *data)
{
	// --------------- START Packing Signal ------------------
	//   startBit                = s.startBit
	//   length                  = s.length
	//   desiredSignalByteLayout = s.dataType
	//   dataType                = s.is_unsigned
	//   factor                  = s.factor
	//   offset                  = s.offset
	//   minimum                 = s.maximum
	//   maximum                 = s.minimum
	//  -----------------------------------------------------------------------
	{
		real64_T outValue = 0;
		{
			real64_T result = value;
			// check the maximum & minimum
			if (fabs(s.minimum - 0.0) > DELTA || fabs(s.maximum - 0.0) > DELTA)
			{
				result = result < s.minimum ? s.minimum : result;
				result = result > s.maximum ? s.maximum : result;
				// if (result < s.minimum) {
				//    // lower saturation
				//   result = s.minimum;
				// }
				//
				// if (result > s.maximum) {
				//    // upper saturation
				//   result = s.maximum;
				// }
			}

			result = (result - s.offset) / s.factor;
			outValue = result;
		}

		int startBit = s.startBit;
		{
			// if the motolora type <BEGENDIAN> the startbit needs to be recalculated
			if (!s.dataType)
			{
				int tmp1 = startBit / 8;
				int tmp2 = tmp1 * 8 + 7 - (startBit % 8) + s.length - 1;
				int tmp3 = tmp2 / 8;
				startBit = tmp3 * 8 + 7 - tmp2 % 8;
			}
		}

		int startIndex = startBit / 8;
		int leftShift = startBit % 8;
		// pack the value by the type
		if (s.is_unsigned)
		{
			long max = pow(2, s.length) - 1;
			long min = 0;
			if (s.length <= 8)
			{
				PACKVALUEUNSIGNED(uint8_T);
			}
			else if (s.length > 8 && s.length <= 16)
			{
				PACKVALUEUNSIGNED(uint16_T);
			}
			else if (s.length > 16 && s.length <= 32)
			{
				PACKVALUEUNSIGNED(uint32_T);
			}
			else if (s.length > 32)
			{
				PACKVALUEUNSIGNED(uint64_T);
			}
		}
		else
		{
			long max = pow(2, s.length) / 2 - 1;
			long min = (-1) * max - 1;
			if (s.length <= 8)
			{
				PACKVALUESIGNED(int8_T);
			}
			else if (s.length > 8 && s.length <= 16)
			{
				PACKVALUESIGNED(int16_T);
			}
			else if (s.length > 16 && s.length <= 32)
			{
				PACKVALUESIGNED(int32_T);
			}
			else if (s.length > 32)
			{
				PACKVALUESIGNED(int64_T);
			}
		}
	}
}

} // namespace can_util
